﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace ControlBox
{
    public class S_FrameParser : IFrameParser
    {
        Robot Robot;
        public S_FrameParser(Robot robot)
        {
            Robot = robot;
        }
        private void Parse(S_Frame frame)
        {

            if (frame.Nck == 01)
            {
                //notify Robot success
            }

            if (frame.Nck == 02)
            {
                //sleep Robot success
            }
            if (frame.Nck == 0)//数据采集帧
            {
                #region aNci数据区解析
                Frame_aNCI[] _aNci = frame.aNci;
                for (int i = 0; i < _aNci.Length; i++)
                {
                    Frame_aNCI aNci = _aNci[i];
                    if (aNci.uCmd != 0)
                    {
                        switch (aNci.uCmd)
                        {
                            case 0x01:
                                Robot.MoveDistance += 10 * aNci.iData;
                                break;
                            case 0x02:
                                Robot.BodyYaw += 10.0f * aNci.iData;
                                break;
                            case 0x03:
                                Robot.HolderLift += 10 * aNci.iData;
                                break;
                            case 0x04:
                                Robot.HolderPitch += 10.0f * aNci.iData;
                                break;
                            case 0x05:
                                Robot.HolderRotate += 10.0f * aNci.iData;
                                break;
                            case 0x06:
                                Robot.HolderLift = 0;
                                Robot.HolderPitch = 0;
                                Robot.HolderRotate = 0;
                                break;
                            case 0x07:
                                if (Robot.CameraSwitch == 0)
                                    Robot.CameraSwitch = 1;
                                else
                                    Robot.CameraSwitch = 0;
                                break;
                            case 0x09:
                                Robot.CameraZoom = aNci.iData;
                                break;
                            case 0x0A:
                                Robot.BrightLevel = aNci.iData;
                                break;
                            case 0x0B:

                                break;
                            case 0x0C:

                                break;
                            case 0x0D:

                                break;
                            case 0x0E:
                                if (aNci.uSub == 0)
                                {

                                }
                                else if (aNci.uSub == 1)
                                {

                                }
                                break;
                            case 0x0F:

                                break;
                            default:
                                break;
                        }
                    }
                }

                #endregion
            }
        }

        public void Parse(Frame frame)
        {
            Parse((S_Frame)frame);
        }
    }
    public class R_FrameParser:IFrameParser
    {
        private Robot robot;

        private int LSBuffer;
        private int leftOverCounts;
        private int leftSpeed;
        private int RSBuffer;
        private int rightOverCounts;
        private int rightSpeed;
        private int holderRotate;
        private int lastHolderRotate;
        private int rotateOver;
        private int rotateBuffer;

        public R_FrameParser(Robot target)
        {
            robot = target; 
        }

        public void Parse(R_Frame recFrame)
        {
            //预存储UDP发送的数据

            //DataManager.LastRecFrame.Nck = recFrame.Nck;
            //DataManager.LastRecFrame.aDI = recFrame.aDI;
            //DataManager.LastRecFrame.aDO = recFrame.aDO;
            //DataManager.LastRecFrame.aRbtCmuInfo = recFrame.aRbtCmuInfo;
            //DataManager.LastRecFrame.unRbtState = recFrame.unRbtState;
            //帧头解析
            if (recFrame.Nck == 01)
            {
                //notify Robot success
            }

            if (recFrame.Nck == 02)
            {
                //sleep Robot success
            }
            if (recFrame.Nck == 0)//数据采集帧
            {
                #region aDI数据区解析
                BitArray _aDIBits = new BitArray(recFrame.aDI.Take(1).ToArray());

                if (_aDIBits[0] == true)//0还是7？？？
                {
                    //2.4G在线
                    robot.NRFState = true;
                }

                if (_aDIBits[1] == true)
                {
                    //2.4G离线
                    robot.NRFState = false;
                }

                if (_aDIBits[2] == true)
                {
                    //2.4G发送故障
                    robot.NRFSendErr = true;
                }

                if (_aDIBits[3] == true)
                {
                    //2.4G接收故障
                    robot.NRFRecErr = true;
                }

                if (_aDIBits[4] == true)
                {
                    //GPS离线
                    robot.GPSState = false;
                }
                else
                {
                    robot.GPSState = false;
                }

                if (recFrame.aDI[1] == 0)
                {
                    //现场操作？？？
                }
                else if (recFrame.aDI[1] == 1)
                {
                    //远程请求
                }
                else if (recFrame.aDI[1] == 2)
                {
                    //远程确认
                }

                //锁？？？
                if (recFrame.aDI[2] == 0)
                {
                    //UNLOCK
                }
                else if (recFrame.aDI[2] == 1)
                {
                    //LOCK
                }

                #endregion
                #region aDO数据区解析
                DataManager.LastRecFrame.aDO = recFrame.aDO;
                Byte[] _aDO = recFrame.aDO;
                //云台，机器人姿态，动作
                robot.CameraSwitch = _aDO[1];//获取摄像头方向
                #endregion
                #region aRbtCmuInfo数据区解析
                //是BitConvert
                DataManager.LastRecFrame.aRbtCmuInfo = recFrame.aRbtCmuInfo;
                Byte[] _aRbtCmuInfo = recFrame.aRbtCmuInfo;

                robot.totalCnt = BitConverter.ToInt16(_aRbtCmuInfo, 0);
                robot.timeoutCnt = BitConverter.ToInt16(_aRbtCmuInfo, 2);
                robot.errCnt = BitConverter.ToInt32(_aRbtCmuInfo, 4);
                robot.rightCnt = BitConverter.ToInt32(_aRbtCmuInfo, 6);
                //为什么没有解析？？？
                //DataManager.rightRate = BitConverter.ToDouble(_aRbtCmuInfo.Skip(8).Take(3));
                //DataManager.errRate = BitConverter.ToDouble(_aRbtCmuInfo.Skip(11).Take(3));
                //DataManager.timeoutRate = BitConverter.ToDouble(_aRbtCmuInfo.Skip(14).Take(3));
                #endregion
                #region unRbtState数据区解析

                #region 行进距离
                Byte[] _unRbtState = recFrame.unRbtState;
                //左右轮速度+管道直径+系数
                //DataManager.moveDistance = BitConverter.ToInt16(_unRbtState, 0);
                int leftPulse = -BitConverter.ToInt16(_unRbtState, 41);
                int leftPulseBuffer = 0;
                if (leftPulse - LSBuffer < -32767)
                {
                    leftOverCounts++;
                }
                if (leftPulse - LSBuffer > 32767)
                {
                    leftOverCounts--;
                }
                if (leftOverCounts < 0)
                {
                    leftPulseBuffer = -(32768 - leftPulse);
                    leftSpeed = 32768 * (2 * leftOverCounts + 1) + leftPulseBuffer;
                }
                if (leftOverCounts > 0)
                {
                    leftPulseBuffer = 32768 + leftPulse;
                    leftSpeed = 32768 * (2 * leftOverCounts - 1) + leftPulseBuffer;
                }
                if (leftOverCounts == 0)
                {
                    leftPulseBuffer = leftPulse;
                    leftSpeed = leftPulseBuffer;
                }
                LSBuffer = leftPulse;
                //right of left wheel
                int rightPulse = -BitConverter.ToInt16(_unRbtState, 43);
                int rightPulseBuffer = 0;
                if (rightPulse - RSBuffer < -32767)
                {
                    rightOverCounts++;
                }
                if (rightPulse - RSBuffer > 32767)
                {
                    rightOverCounts--;
                }
                if (rightOverCounts < 0)
                {
                    rightPulseBuffer = -(32768 - rightPulse);
                    rightSpeed = 32768 * (2 * rightOverCounts + 1) + rightPulseBuffer;
                }
                if (rightOverCounts > 0)
                {
                    rightPulseBuffer = 32768 + rightPulse;
                    rightSpeed = 32768 * (2 * rightOverCounts - 1) + rightPulseBuffer;
                }
                if (rightOverCounts == 0)
                {
                    rightPulseBuffer = rightPulse;
                    rightSpeed = rightPulseBuffer;
                }
                RSBuffer = rightPulse;
                //radius of the wheel
                int radius = int.Parse(ConfigHandler.Config["PipeRadius"]);
                if (radius == 0)
                {
                    // 请求数据库？？？
                    // radius = Double.parseDouble(params[2]);
                    radius = int.Parse(ConfigHandler.Config["WheelRadius"]);
                }
                //move distance
                robot.MoveDistance = (float)(((leftSpeed + rightSpeed) / (2 * 58 * 3 * 4.5)) * (Math.PI * 2 * radius)) / 1000.0f * float.Parse(ConfigHandler.Config["DistanceQuotiety"]);
                //distance = Double.parseDouble(String.format("%.1f", distance));
                #endregion

                //在视频上显示机器人行驶速度
                robot.MoveSpeed = BitConverter.ToInt16(_unRbtState, 2);

                robot.BodyRoll = (float)(BitConverter.ToInt16(_unRbtState, 4) / 10.0f);

                robot.BodyPitch = (float)(BitConverter.ToInt16(_unRbtState, 6) / 10.0f);

                robot.BodyYaw = (float)(BitConverter.ToInt16(_unRbtState, 8) / 10.0f);
                //云台举升
                robot.HolderLift = BitConverter.ToInt16(_unRbtState, 10) / 10;
                //云台旋转
                holderRotate = BitConverter.ToInt16(_unRbtState, 12);
                //要计算差值（记录圈数）
                if (holderRotate - lastHolderRotate < -50000)
                {
                    rotateOver++;
                    //顺时针旋转越界
                }
                if (holderRotate - lastHolderRotate > 50000)
                {
                    rotateOver--;
                    //逆时针旋转越界
                }
                if (Math.Abs(holderRotate - lastHolderRotate) > 20000 && Math.Abs(holderRotate - lastHolderRotate) < 50000)
                {
                    rotateOver = 0;
                    //经过原点
                }
                if (rotateOver < 0)
                {
                    rotateBuffer = -(32768 - holderRotate);
                    robot.HolderRotate = (32768 * (2 * rotateOver + 1) + rotateBuffer) / 100;
                }
                if (rotateOver > 0)
                {
                    rotateBuffer = 32768 + holderRotate;
                    robot.HolderRotate = (32768 * (2 * rotateOver - 1) + rotateBuffer) / 100;
                }
                if (rotateOver == 0)
                {
                    robot.HolderRotate = holderRotate / 100;
                }
                lastHolderRotate = holderRotate;

                robot.HolderPitch = (float)(BitConverter.ToInt16(_unRbtState, 14) / 100.0);
                robot.HolderAtmos = (float)(BitConverter.ToInt16(_unRbtState, 16) / 100);
                robot.HolderTemp = (float)(BitConverter.ToInt16(_unRbtState, 18) / 100);
                robot.HolderHumidity = (int)(BitConverter.ToInt16(_unRbtState, 20) / 100);

                robot.CameraZoom = (int)(_unRbtState[22]);
                robot.BrightLevel = BitConverter.ToInt16(_unRbtState, 23);


                //GPS数据解析
                robot.Nshemi = (char)(_unRbtState[29]);
                if (robot.Nshemi == 'S')
                {
                    robot.GPSLatitude = -(float)(BitConverter.ToSingle(_unRbtState, 25) / 100000.0f);
                }
                else if (robot.Nshemi == 'N')
                {
                    robot.GPSLatitude = (float)(BitConverter.ToSingle(_unRbtState, 25) / 100000.0f);
                }
                robot.Ewhemi = (char)(_unRbtState[34]);
                if (robot.Ewhemi == 'W')
                {
                    robot.GPSLngitude = -(float)(BitConverter.ToSingle(_unRbtState, 30) / 100000.0f);
                }
                else if (robot.Ewhemi == 'E')
                {
                    robot.GPSLngitude = (float)(BitConverter.ToSingle(_unRbtState, 30) / 100000.0f);
                }


                short power = BitConverter.ToInt16(_unRbtState, 35);
                //if (power != 0)
                //{
                    robot.BatLevel = (float)power / 10.0f;
                //}

                if (robot.BatLevel < 22.5)
                {
                    robot.lowPowerWarning = true;
                }
                else
                {
                    robot.lowPowerWarning = false;
                }


                robot.BodyAtmos = (float)(BitConverter.ToInt16(_unRbtState, 37) / 10.0f);

                robot.BodyTemp = (float)(BitConverter.ToInt16(_unRbtState, 39) / 10.0f);
                #endregion
                #region unSnrData数据区解析（暂无）

                #endregion
                #region unPipeData数据区解析（暂无）

                #endregion
                #region aNci数据区解析
                Frame_aNCI[] _aNci = recFrame.aNci;
                for (int i = 0; i < _aNci.Length; i++)
                {
                    if (_aNci[i].uCmd == 0x0E)
                    {
                        if (_aNci[i].uSub == 0x01)//气体
                        {
                            if (_aNci[i].uExt1 == 0x01)
                            {
                                robot.CO = BitConverter.ToInt16(_aNci[i].ByteArray, 4);
                              //  robot.handler.UI.UpdateList("获取CO浓度：" + (double)(robot.gasSensor.CO / 100.00) + " PPM");
                                robot.H2S = BitConverter.ToInt16(_aNci[i].ByteArray, 6);
                               // robot.handler.UI.UpdateList("获取H2S浓度：" + (double)(robot.gasSensor.H2S / 100.00) + " PPM");
                            }
                            if (_aNci[i].uExt1 == 0x02)
                            {
                                robot.NH3 = BitConverter.ToInt16(_aNci[i].ByteArray, 4);
                               // robot.handler.UI.UpdateList("获取NH3浓度：" + (double)(robot.gasSensor.NH3 / 100.00) + " PPM");
                                robot.O2 = BitConverter.ToInt16(_aNci[i].ByteArray, 6);
                               // robot.handler.UI.UpdateList("获取O2浓度：" + (double)(robot.gasSensor.O2 / 100.00) + " PPM");
                            }
                            if (_aNci[i].uExt1 == 0x03)
                            {
                                robot.CO2 = BitConverter.ToInt16(_aNci[i].ByteArray, 4);
                               // robot.handler.UI.UpdateList("获取CO2浓度：" + (double)(robot.gasSensor.CO2 / 100.00) + " PPM");
                                robot.CH4 = BitConverter.ToInt16(_aNci[i].ByteArray, 6);
                                //robot.handler.UI.UpdateList("获取CH4浓度：" + (double)(robot.gasSensor.CH4 / 100.00) + " PPM");
                            }

                        }
                        else if (_aNci[i].uSub == 0x02)//激光
                        {
                            if (_aNci[i].uExt1 == 0x01)
                            {
                                int laser2 = BitConverter.ToInt16(_aNci[i].ByteArray, 4);
                                //robot.laser.LaserData.Add(20, laser2);
                              //  robot.handler.UI.UpdateList("获取激光#2数据：" + laser2);
                                int laser1 = BitConverter.ToInt16(_aNci[i].ByteArray, 6);
                                // Laser.Add(10, laser1);
                               // robot.handler.UI.UpdateList("获取激光#1数据：" + laser1);
                            }
                            if (_aNci[i].uExt1 == 0x02)
                            {
                                int laser4 = BitConverter.ToInt16(_aNci[i].ByteArray, 4);
                                // Laser.Add(40, laser4);
                                //robot.handler.UI.UpdateList("获取激光#4数据：" + laser4);
                                int laser3 = BitConverter.ToInt16(_aNci[i].ByteArray, 6);
                                // Laser.Add(30, laser3);
                               // robot.handler.UI.UpdateList("获取激光#3数据：" + laser3);
                            }
                            if (_aNci[i].uExt1 == 0x03)
                            {
                                int laser6 = BitConverter.ToInt16(_aNci[i].ByteArray, 4);
                                //   Laser.Add(60, laser6);
                                //robot.handler.UI.UpdateList("获取激光#6数据：" + laser6);
                                int laser5 = BitConverter.ToInt16(_aNci[i].ByteArray, 6);
                                // Laser.Add(50, laser5);
                               // robot.handler.UI.UpdateList("获取激光#5数据：" + laser5);
                            }
                            if (_aNci[i].uExt1 == 0x04)
                            {
                                int laser8 = BitConverter.ToInt16(_aNci[i].ByteArray, 4);
                                // Laser.Add(80, laser8);
                                //robot.handler.UI.UpdateList("获取激光#8数据：" + laser8);
                                int laser7 = BitConverter.ToInt16(_aNci[i].ByteArray, 6);
                                //  Laser.Add(70, laser7);
                                //robot.handler.UI.UpdateList("获取激光#7数据：" + laser7);
                            }
                            if (_aNci[i].uExt1 == 0x05)
                            {
                                int laser10 = BitConverter.ToInt16(_aNci[i].ByteArray, 4);
                                //   Laser.Add(-20, laser10);
                                //robot.handler.UI.UpdateList("获取激光#10数据：" + laser10);
                                int laser9 = BitConverter.ToInt16(_aNci[i].ByteArray, 6);
                                // Laser.Add(-10, laser9);
                                //robot.handler.UI.UpdateList("获取激光#9数据：" + laser9);
                            }
                            if (_aNci[i].uExt1 == 0x06)
                            {
                                int laser12 = BitConverter.ToInt16(_aNci[i].ByteArray, 4);
                                // Laser.Add(-40, laser12);
                                //robot.handler.UI.UpdateList("获取激光#12数据：" + laser12);
                                int laser11 = BitConverter.ToInt16(_aNci[i].ByteArray, 6);
                                //  Laser.Add(-30, laser11);
                                //robot.handler.UI.UpdateList("获取激光#11数据：" + laser11);
                            }
                            if (_aNci[i].uExt1 == 0x07)
                            {
                                int laser14 = BitConverter.ToInt16(_aNci[i].ByteArray, 4);
                                //  Laser.Add(-60, laser14);
                                //robot.handler.UI.UpdateList("获取激光#14数据：" + laser14);
                                int laser13 = BitConverter.ToInt16(_aNci[i].ByteArray, 6);
                                //  Laser.Add(-50, laser13);
                                //robot.handler.UI.UpdateList("获取激光#13数据：" + laser13);
                            }
                            if (_aNci[i].uExt1 == 0x08)
                            {
                                int laser16 = BitConverter.ToInt16(_aNci[i].ByteArray, 4);
                                //Laser.Add(-80, laser16);
                                //robot.handler.UI.UpdateList("获取激光#16数据：" + laser16);
                                int laser15 = BitConverter.ToInt16(_aNci[i].ByteArray, 6);
                                //  Laser.Add(-70, laser15);
                                //robot.handler.UI.UpdateList("获取激光#15数据：" + laser15);
                            }

                        }
                    }
                }
                #endregion
            }
        }

        public void Parse(Frame frame)
        {
            Parse((R_Frame)frame);
        }

    }

    /// <summary>
    /// 解析器
    /// 将UDP收到的aNci转交给Serial发送
    /// </summary>
    public class Transmit : IFrameParser
    {
        private SerialDataFactory serialHandler;

        public Transmit(SerialDataFactory _serialHandler)
        {
            serialHandler = _serialHandler;
        }

        public void Parse(Frame frame)
        {
            Parse((S_Frame)frame);
        }
        private void Parse(S_Frame frame)
        {
            if (frame.Nck == 0)//数据采集帧
            {
                #region aNci数据区解析
                Frame_aNCI[] _aNci = frame.aNci;
                for (int i = 0; i < _aNci.Length; i++)
                {
                    Frame_aNCI aNci = _aNci[i];
                    if (aNci.uCmd != 0)
                    {
                        serialHandler.EnqueueaNci(aNci);
                    }
                }
                #endregion
            }
        }
    }
}
